#ifndef PHIDGET_ALREADY
#define PHIDGET_ALREADY
#include "phidget21.h"
#endif
#include <iostream>
#include "grippercontroller.h"
#include "elbowcontroller.h"
#include "rackcontroller.h"
#include "shouldercontroller.h"


#define SERVO_SERIAL 176765
#define STEPPER_SERIAL 269688
#define MOTOR_SERIAL 146782


using namespace std;

int CCONV ErrorHandler(CPhidgetHandle ADVSERVO, void *userptr, int ErrorCode, const char *Description)
{
	cerr << "Error: " << ErrorCode << ", " <<  Description << endl;
	return 0;
}


class RobotController {

public:
	CPhidgetAdvancedServoHandle servo;
	CPhidgetStepperHandle stepper;
	CPhidgetMotorControlHandle motor;

	GripperController gripper;
	ElbowController elbow;
	RackController rack;
	ShoulderController shoulder;
	
	RobotController()  {
		int result;
		const char *err;	// some constants we use for error reporting



		// set up stepper board
		CPhidgetStepper_create(&stepper);
		CPhidget_set_OnError_Handler((CPhidgetHandle)stepper, ErrorHandler, NULL);
		CPhidget_open((CPhidgetHandle)stepper, STEPPER_SERIAL);
		cerr << "Waiting for stepper board to be attached...\n";
		if((result = CPhidget_waitForAttachment((CPhidgetHandle)stepper, 10000))) {
			CPhidget_getErrorDescription(result, &err);
			cerr << "Problem waiting for attachment: " << err << endl;
			return;
		}

		// initialize elbow controller
		elbow.init(stepper);



		// set up DC motor board
		CPhidgetMotorControl_create(&motor);
		CPhidget_set_OnError_Handler((CPhidgetHandle)motor, ErrorHandler, NULL);
		CPhidget_open((CPhidgetHandle)motor, MOTOR_SERIAL);
		cerr << "Waiting for DC motor board to be attached...\n";
		if((result = CPhidget_waitForAttachment((CPhidgetHandle)motor, 10000))) {
			CPhidget_getErrorDescription(result, &err);
			cerr << "Problem waiting for attachment: " << err << endl;
			return;
		}

		// initialize shoulder controller
		shoulder.init(motor);



		// set up servo board
		CPhidgetAdvancedServo_create(&servo);
		CPhidget_set_OnError_Handler((CPhidgetHandle)servo, ErrorHandler, NULL);
		CPhidget_open((CPhidgetHandle)servo, SERVO_SERIAL);
		cerr << "Waiting for servo board to be attached...\n";
		if((result = CPhidget_waitForAttachment((CPhidgetHandle)servo, 10000))) {
			CPhidget_getErrorDescription(result, &err);
			cerr << "Problem waiting for attachment: " << err << endl;
			return;
		}

		// initialize gripper controller and rack controller
		gripper.init(servo);
		rack.init(servo);



		return;
	}

	// when the program is closed, this will disengage all motors and delete Phidgets handlers
	~RobotController() {
		elbow.disengage();
		CPhidget_close((CPhidgetHandle)stepper);
		CPhidget_delete((CPhidgetHandle)stepper);

		gripper.disengage();
		rack.disengage();
		CPhidget_close((CPhidgetHandle)servo);
		CPhidget_delete((CPhidgetHandle)servo);

		shoulder.disengage();
		CPhidget_close((CPhidgetHandle)motor);
		CPhidget_delete((CPhidgetHandle)motor);
	}
};